import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class Wallfollow{

        float desiredDistance = 37f;
//      private float diff;
        private int side;
//      private float delta;
        private Pilot p;
        private int sleepTimer = 100;
        Thread t;
        Thread wft;
//      Thread d;
        private Master master;
        private Driver goldieDriver;
        private boolean run =true;
        Float moveSpeed;


        public Wallfollow(Master master, Driver goldieDriver, boolean left, Float moveSpeed){
                this.p = p;     
                this.master = master;
                this.goldieDriver = goldieDriver;
                this.moveSpeed = moveSpeed;
                wft = new Thread(new WallFollowThread());
                this.setSideLeft(left);
                wft.start();
//              d.start();
        }

        class WallFollowThread implements Runnable {
                public void run(){
                        float diff;
                        float delta;
                        float firstDistance = 0;
                        float lastDiff =0;
                        float previousCorrect = 0;
                        float alfa = 0.8f;
                        while (true){

                                try{
                                        Thread.sleep(50);
                                }catch(InterruptedException e){
                                }
                                while(run){
                                        float secondDistance = master.getDistance();    
                                        //factor voor afgeleide afstand in de tijd
                                        diff = 100/sleepTimer*(secondDistance - firstDistance)*16f/moveSpeed*30f;//12
                                        //factor voor afstand tot muur in de tijd
                                        delta = (secondDistance - desiredDistance)*1.33f;//1.33
                                        firstDistance = secondDistance;
//                                        if(Math.abs(diff)>300){
//                                                diff = 0;
//                                        }
//                                        else{
//                                                lastDiff= diff;
//                                        }
//
//                                        if(Math.abs(delta)>100){
//                                                delta = 40*Math.signum(delta);
//
//                                        }
//                                        else{
//                                                lastDiff= diff;
//                                        }
                                        //voorwaarde oop diff+delta maken!
                                        float total = diff+delta;
                                        if(Math.abs(total)>90)
                                        	total=Math.signum(total)*90;
                                        float correct = total*side;

                                        if (run)
                                                goldieDriver.addCommand(new Command(correct*alfa+previousCorrect*(1-alfa), 45f, false, CommandEnum.STEER));
                                        previousCorrect = correct;
                                        try{
                                                Thread.sleep(sleepTimer);
                                        }catch(InterruptedException e){
                                        }
                                }
                        }
                }
        


//      class Diff implements Runnable{
//              public void run(){
//                      float first = 0;
//                      float lastDiff =0;
//                      while(true){
//                              try{
//                                      Thread.sleep(sleepTimer);
//                              }catch(InterruptedException e){
//                              }
//                              float second = mast.getDistance();      
//                              diff = 100/sleepTimer*(second - first)*12f;
//                              delta = (second - distance)*1f;
//                              first = second;
//                              //grote diff niet doorgeven
//                              float filteredDiff = diff;
//                              if(Math.abs(diff)>300){
//                                      diff = 0;
//
//                              }
//                              else{
//                                      lastDiff= diff;
//                              }
//
//                              if(Math.abs(delta)>100){
//                                      delta = 40*Math.signum(delta);
//
//                              }
//                              else{
//                                      lastDiff= diff;
//                              }
//
//
//                      }
//              }
        }


//      public float getDifference(){
//              return this.diff;
//      }

        /**
         * Bepaalt de draairichting van de robot
         * @param       left
         *                      if(true)
         *                              muur staat links
         *                      else
         *                              muur staat rechts
         */
        public void setSideLeft(boolean left){
                if(left)
                        this.side = 1;
                else
                        this.side = -1;
        }
        public boolean getSideLeft(){
                return side==1;
        }
        public int getSide(){
                return side;
        }

        public void pause(){
                run = false;
        }

        public void restart(){
                run = true;
        }


        public void setDistance(float distance){
                desiredDistance = distance;
        }

}